#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from maindecte_srvs.srv import maindecte, maindecteRequest, maindecteResponse
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
import cv2
import signal
import time

'''
 * 海康摄像头 测试demo
 * 苏凯
 * 2023-03-28
 *  rosrun maindecte maindecte_text.py
'''
FilePath=None
filepath=None
OPT=None
task_id=None
sensor_id=None
x=None
y=None
dx=None
dy=None
resize_cot=None
resize_cot=None
msg=None
fPan=None
fTilt=None
fZoom=None
dwFocus=None
aperture=None
shutter=None
exposure=None
gain=None
data=None
CMD=None
Parameter=None
Direction=None
Speedlevel=None


#操作云台
start =0 #开启
stop =1  #关闭
# dwPTZCommand 含义
LIGHT_PWRON =2 #接通灯光电源
WIPER_PWRON =3 #接通雨刷开关
FAN_PWRON   =4 #接通风扇开关
HEATER_PWRON= 5 #接通加热器开关
AUX_PWRON1 =6 #接通辅助设备开关
AUX_PWRON2= 7 #接通辅助设备开关
ZOOM_IN =11 #焦距变大(倍率变大)67
#设备网络 SDK 编程指南
ZOOM_OUT =12 #焦距变小(倍率变小)
FOCUS_NEAR =13 #焦点前调
FOCUS_FAR =14 #焦点后调
IRIS_OPEN =15 #光圈扩大
IRIS_CLOSE =16 #光圈缩小
TILT_UP= 21 #云台上仰
TILT_DOWN =22 #云台下俯
PAN_LEFT= 23 #云台左转
PAN_RIGHT =24 #云台右转
UP_LEFT =25 #云台上仰和左转
UP_RIGHT =26 #云台上仰和右转
DOWN_LEFT =27 #云台下俯和左转
DOWN_RIGHT= 28 #云台下俯和右转
PAN_AUTO =29 #云台左右自动扫描


# 监听ctrl c
def my_handler(signum, frame):
    #关闭节点
    rospy.signal_shutdown("ctrl c!")
    if rospy.is_shutdown():
        print("close ros  py_maindecte_text_node")
        #cv2.colse()
        cv2.destroyAllWindows()
    print("终止")

def myTimer_callback(event):
    print("  py_myTimer_callback 定时执行  ")


def callback_image(data):
    # Initialize CvBridge
    bridge = CvBridge()

    # Convert ROS Image message to OpenCV format
    cv_image = bridge.imgmsg_to_cv2(data, desired_encoding="passthrough")

    # Display the resulting OpenCV image
    cv2.imshow("Image window", cv_image)
    cv2.waitKey(1)

#操作海康摄像头
def maindecte_service():
    request = maindecteRequest()
    request.msgtype=43 #获取相机参数 相机位置
    response = client.call(request)
    #是否类型一致
    if isinstance(response, maindecteResponse):
        if response.result == 'ok':
                    global  FilePath ,filepath ,OPT ,task_id ,sensor_id ,x ,y ,dx, dy ,resize_cotX, resize_cotY, msg ,fPan, fTilt, fZoom ,dwFocus, aperture, shutter ,exposure, gain ,data, CMD ,Parameter, Direction ,Speedlevel
                    FilePath=response.result_msgdata.FilePath
                    filepath=response.result_msgdata.filepath
                    OPT=response.result_msgdata.OPT
                    task_id=response.result_msgdata.task_id
                    sensor_id=response.result_msgdata.sensor_id
                    x=response.result_msgdata.x
                    y=response.result_msgdata.y
                    dx=response.result_msgdata.dx
                    dy=response.result_msgdata.dy
                    resize_cotX=response.result_msgdata.resize_cotX
                    resize_cotY=response.result_msgdata.resize_cotY
                    msg=response.result_msgdata.msg
                    fPan=response.result_msgdata.fPan
                    fTilt=response.result_msgdata.fTilt
                    fZoom=response.result_msgdata.fZoom
                    dwFocus=response.result_msgdata.dwFocus
                    aperture=response.result_msgdata.aperture
                    shutter=response.result_msgdata.shutter
                    exposure=response.result_msgdata.exposure
                    gain=response.result_msgdata.gain
                    data=response.result_msgdata.data
                    CMD=response.result_msgdata.CMD
                    Parameter=response.result_msgdata.Parameter
                    Direction=response.result_msgdata.Direction
                    Speedlevel=response.result_msgdata.Speedlevel
                    print("maindecte_service  ok")
        else:
            print("maindecte_service  error")
#操作海康摄像头
def maindecte_service2():
    request = maindecteRequest()
    request.msgtype=43 #获取相机参数 相机位置
    response = client.call(request)
    #是否类型一致
    if isinstance(response, maindecteResponse):
        if response.result == 'ok':
            print("maindecte_service  ok")
            return response.result_msgdata
        else:
            print("maindecte_service  error")
            return None
#操作海康云台 500001
def maindecte_service1(msgtype,dwPTZCommand,dwStop):
    request = maindecteRequest()
    request.msgtype=msgtype #获取相机参数 云台控制
    request.dwPTZCommand=dwPTZCommand   #云台控制命令
    request.dwStop=dwStop        #0开始， 1停止
    print(TILT_UP)
    response = client.call(request)
    #是否类型一致
    if isinstance(response, maindecteResponse):
        if response.result == 'ok':
            print("maindecte_service 云台控制  ok")
        else:
            print("maindecte_service 云台控制 error")



#操作海康云台 带速度  500002
def maindecte_serviceSpeed(msgtype,dwPTZCommand,dwStop,dwSpeed):
    request = maindecteRequest()
    request.msgtype=msgtype #获取相机参数 云台控制
    request.dwPTZCommand=dwPTZCommand   #云台控制命令
    request.dwStop=dwStop        #0开始， 1停止
    request.dwSpeed=dwSpeed       #速度 范围（1-7）
    print(TILT_UP)
    response = client.call(request)
    #是否类型一致
    if isinstance(response, maindecteResponse):
        if response.result == 'ok':
            print("maindecte_service 云台控制  ok")
        else:
            print("maindecte_service 云台控制 error")

#操作车辆
def cmdVelF():
    cmd_vel = Twist()
    cmd_vel.linear.x = 0.1  # 设置线速度 负数为倒车
    cmd_vel.angular.z = 0.2  # 设置角速度 负数为顺时针旋转
    cmdVelpub.publish(cmd_vel)  # 发布消息

# rosrun maindecte maindecte_text.py
if __name__ == "__main__":
    #rospy.init_node('py_start_node',anonymous=True)
    rospy.init_node('py_maindecte_text_node')
    # 监听ctrl c
    signal.signal(signal.SIGINT, my_handler)    #读取Ctrl+c信号

    #操作车辆
    cmdVelpub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    # 创建Service Client 操作海康相机
    serviceName = "/maindecte_service"
    client = rospy.ServiceProxy(serviceName, maindecte)
    # 等待服务开启
    rospy.wait_for_service(serviceName)

    #定时执行 https://blog.csdn.net/qq_41943585/article/details/91041964 , 单发 （oneshot）指定计时器是否为一次性计时器。如果是这样，它只会触发一次。否则会不断地重新调度，直到停止。
    rospy.Timer(rospy.Duration(2),myTimer_callback,oneshot=True)

    # Subscribe to the camera topic
    rospy.Subscriber("/hikrobot/camera/image", Image, callback_image)
    '''
    #控制云台 函数
    #开启云台运动
    maindecte_serviceSpeed(500002,TILT_UP,start,1) #云台控制命令  0开始， 1停止  速度 范围（1-7）
    time1 = time.time()
    time.sleep(0.1) #延时0.1秒
    #关闭云台运动
    maindecte_serviceSpeed(500002,TILT_UP,stop,1) #云台控制命令  0开始， 1停止  速度 范围（1-7）
    
    '''



    # 事件处理
    rospy.spin()
    # Close all windows
    cv2.destroyAllWindows()




